#include "Simple_Sensor.h"
#include "foc_utils.h"
#include "Simple_time.h"

void Simple_Sensor_update(SimpleSensor_t *handle)
{
    float val = handle->function.getSensorAngle();
    handle->state.angle_prev_ts = Simple_micros();
    float d_angle = val - handle->state.angle_prev;
    // if overflow happened track it as full rotation
    if(abs(d_angle) > (0.8f*_2PI) ) 
      handle->state.full_rotations += ( d_angle > 0 ) ? -1 : 1; 
    handle->state.angle_prev = val;
}


 /** get current angular velocity (rad/s) */
float Simple_Sensor_getVelocity(SimpleSensor_t *handle) 
{
	return handle->function.getVelocity();
//    // calculate sample time
//    float Ts = (handle->state.angle_prev_ts - handle->state.vel_angle_prev_ts)*1e-6;
//    // TODO handle overflow - we do need to reset vel_angle_prev_ts
//    if (Ts <handle->min_elapsed_time) 
//       return handle->state.velocity; // don't update velocity if deltaT is too small

//    handle->state.velocity = ( (float)(handle->state.full_rotations - handle->state.vel_full_rotations)*_2PI + (handle->state.angle_prev - handle->state.vel_angle_prev) ) / Ts;
//    handle->state.vel_angle_prev = handle->state.angle_prev;
//    handle->state.vel_full_rotations = handle->state.full_rotations;
//    handle->state.vel_angle_prev_ts = handle->state.angle_prev_ts;
//    return handle->state.velocity;
}


void SimpleSensor_init(SimpleSensor_t *handle) 
{
    // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero)
    handle->function.getSensorAngle(); // call once
    Simple_DelayMicroseconds(1);
    handle->state.vel_angle_prev = handle->function.getSensorAngle(); // call again
    handle->state.vel_angle_prev_ts = Simple_micros();
    Simple_Delay(1);
    handle->function.getSensorAngle(); // call once
    Simple_DelayMicroseconds(1);
    handle->state.angle_prev = handle->function.getSensorAngle(); // call again
    handle->state.angle_prev_ts = Simple_micros();
}

void SimpleSensor_FunctionRegister(SimpleSensor_t *handle,SimpleSensorFunction_t f)
{
	handle->function=f;
}

float Simple_Sensor_getMechanicalAngle(SimpleSensor_t *handle) 
{
	  return handle->function.getMechanicalAngle();
   // return  handle->state.angle_prev;
}


float Simple_Sensor_getAngle(SimpleSensor_t *handle)
{
	  return handle->function.getSensorAngle();
    //return (float)handle->state.full_rotations * _2PI + handle->state.angle_prev;
}


double Simple_Sensor_getPreciseAngle(SimpleSensor_t *handle)
{
	  return handle->function.getPreciseAngle();
    //return (double) handle->state.full_rotations * (double)_2PI + (double)handle->state.angle_prev;
}


int32_t Simple_Sensor_getFullRotations(SimpleSensor_t *handle) 
{
	  return handle->function.getFullRotations();
    //return handle->state.full_rotations;
}



int Simple_Sensor_needsSearch(SimpleSensor_t *handle) 
{
    return 0; // default false
}
